#ifndef FIBONACCIHEAP_H
#define FIBONACCIHEAP_H

#include "NodeList.h"
#include "PriorityQueue.h"
#include "math.h"
#include "string.h"

namespace algo {

  inline size_t ilog2(size_t x) 
  {
    size_t l=0;
    if(x >= 1<<16) { x>>=16; l|=16; }
    if(x >= 1<<8) { x>>=8; l|=8; }
    if(x >= 1<<4) { x>>=4; l|=4; }
    if(x >= 1<<2) { x>>=2; l|=2; }
    if(x >= 1<<1) l|=1;
    return l;
  }

  template<typename T>
  class FibonacciHeap;

  /**
   * The FibonacciHeapNode is the internal node used for each key in the FibonacciHeap.
   * It's designed as being the node in a double linked list by extending NodeListElement.
   */
  template<typename T>
  class FibonacciHeapNode : public PriorityQueueNode<T>,
                            public NodeListElement< FibonacciHeapNode<T> > {
  public:
    /**
     * Get the key associated with the node
     */
    T getKey() const { return key; }

    std::string dotString() const {
      std::ostringstream res;
      res << (size_t)this << " [label=\"" << "somekey" << " - m: " << marked() << "\"]\n";

      if (this->next)
        res << (size_t)this << " -> " << (size_t)this->next << "\n";
      if (this->prev)
        res << (size_t)this << " -> " << (size_t)this->prev << "\n";

      if (parent())
        res << (size_t)this << " -> " << (size_t)parent() << "\n";

      FibonacciHeapNode<T> *element = nodes.front();
      while(element) {
        res << element->dotString();
        element = element->next;
      }

      return res.str();
    }

  private:
    FibonacciHeapNode(T key) : key(key), _parent(0), _marked(0) {}

    /**
     * Return the rank of the node. This is equal to the number of child nodes.
     */
    size_t getRank() const { return nodes.size(); /* FIXME */ }

    FibonacciHeapNode<T> *addChild(FibonacciHeapNode<T> *node) {
      nodes.push_back(node);
      node->parent(this);
      return this;
    }


    /**
     * Save a few extra bytes by using a packed pointer, that takes advantage of
     * word-aligned pointers. The marked bit is saved in the LSB of parent.
     * This optimization is a bit overkill since the type is too large to be in 
     * a register anyway. Buhu :(
     */
    FibonacciHeapNode<T> *parent() const { 
      return _parent;
//      return (FibonacciHeapNode<T> *)((size_t)_parent & ~(size_t)1); 
    }
    void parent(FibonacciHeapNode<T> *p)  { 
      _parent = p;
//      _parent = (FibonacciHeapNode<T> *)((size_t)p & ~(size_t)marked()); 
    }

    bool marked() const { 
      return _marked;
//      return (size_t)_parent & (size_t)1; 
    }
    void marked(bool m) { 
      _marked = m;
//      _parent = (FibonacciHeapNode<T> *)((size_t)_parent | (size_t)m); 
    }
    

  private:
    friend class FibonacciHeap<T>;

    /// The key. It's required that this type is comparable with '<' and the like.
    T key;
    /*void **/ FibonacciHeapNode<T> *_parent;
    bool _marked;
    NodeList< FibonacciHeapNode<T> > nodes;
  };

  template<typename T>
  class FibonacciHeap : public PriorityQueue<T> {
  private:

  public:
    FibonacciHeap() : PriorityQueue<T>("FibonacciHeap"), min_node(0), count(0) {}

    /**
     * Just return the min node. This is either the minimum node or Null
     */
    FibonacciHeapNode<T> *getMin() const { return min_node; }

    /*
     * Insert an new key into the Heap. It returns a new Node-object associated
     * to the key. This node is required as argument in deleteNode/deleteMin/decreaseKey.
     */
    FibonacciHeapNode<T> *insert(T key) {
      FibonacciHeapNode<T> *newNode = new FibonacciHeapNode<T>(key);
      insertNode(newNode);
      ++count;
      return newNode;
    }

    /**
     * Merge two trees. The other tree will be broken and the tree being invoked on will
     * be the new combined tree.
     */
    void merge(const FibonacciHeap<T> &other) {
      count += other.count;
      if (min_node && other.min_node)
        min_node = min_node->key < other.min_node->key ? min_node : other.min_node;
      else if (other.min_node)
        min_node = other.min_node;

      nodes.push_back(other.nodes);
    }

    void decreaseKey(PriorityQueueNode<T> *node, T newKey) {
      decreaseKey(static_cast<FibonacciHeapNode<T> *>(node), newKey);
    }

    void decreaseKey(FibonacciHeapNode<T> *node, T newKey) {
      node->key = newKey;

      if (!node->parent()) {
        min_node = min_node->key < node->key ? min_node : node;
        return;
      }

      if (node->parent()->key <= node->key)
        return;

      cutNode(node);
    } 

    T deleteMin() {
      if (!min_node)
        return T();

      T key = min_node->key;

      nodes.remove(min_node);
      nodes.push_back(min_node->nodes);

      // Use faster integer-log2 method. +2 for the sake of "rounding".
      size_t max_rank = ilog2(count) + 2;

      // Use on-stack allocated array. Save a really expensive new/delete call!
#if WIN32 || WIN64
      FibonacciHeapNode<T> **node_list = new FibonacciHeapNode<T>*[max_rank];
#else
      FibonacciHeapNode<T> *node_list[max_rank];
#endif
      memset(node_list, 0, max_rank * sizeof(FibonacciHeapNode<T>*));

      FibonacciHeapNode<T> *next, *node = nodes.front();

      while (node != 0) {
        node->parent(0);
        next = node->next;
        addNode(node, node_list);
        node = next;
      }

      nodes.clear();
      delete min_node;
      min_node = 0;
      for (size_t i = 0; i < max_rank; i++) {
        if (node_list[i])
          insertNode(node_list[i]);
      }

      --count;
      return key;
    }

    void deleteNode(PriorityQueueNode<T> *node) {
      decreaseKey(dynamic_cast<FibonacciHeapNode<T>*>(node), T(0));
      deleteMin();
    }


    std::string dotString() const {
      std::ostringstream res;
      res << "digraph FibonacciHeap {\nrankdir=BT \n";
      FibonacciHeapNode<T> *element = nodes.front();
      while(element) {
        res << (size_t)element << " -> " << (size_t)this << "\n";
        res << element->dotString();
        element = element->next;
      }
      res << (size_t)this << " [label=\"FibonacciHeap\"]\n";
      res << "}\n";
      return res.str();
    }

  private:
    void cutNode(FibonacciHeapNode<T> *node) {
      node->marked(false);

      FibonacciHeapNode<T> *parent = node->parent();

      if (!parent)
        return;

      custom_count++;
      node->parent(0);
      parent->nodes.remove(node);
      nodes.push_back(node);
      min_node = min_node->key <= node->key ? min_node : node;

      if (!parent->marked()) {
        if (parent->parent())
          parent->marked(true);
        return;
      }
      cutNode(parent);
    }

    void insertNode(FibonacciHeapNode<T> *newNode) {
      if (min_node)
        min_node = min_node->key <= newNode->key ? min_node : newNode;
      else
        min_node = newNode;
      nodes.push_back(newNode);
    }

    void addNode(FibonacciHeapNode<T> *node, FibonacciHeapNode<T> **node_list) {
      size_t rank = node->getRank();

      if (node_list[rank] != 0) {
        FibonacciHeapNode<T> *newNode = node_list[rank];
        if (newNode->key < node->key)
          newNode = newNode->addChild(node);
        else
          newNode = node->addChild(newNode);
        node_list[rank] = 0;
        addNode(newNode, node_list);
      }
      else {
        node_list[rank] = node;
      }
    }

  private:
    NodeList< FibonacciHeapNode<T> > nodes;
    FibonacciHeapNode<T> *min_node;
    size_t count;
  };
}

#endif //FIBONACCIHEAP_H
